#include "Calibrate.h"
#include "MPU6050.h"
#include "OLED.h"
#include <stddef.h>   // 添加此头文件以定义 NULL
#include "Store.h"

// 陀螺仪零偏校准函数
void CalibrateGyro(float *gyroXOffset, float *gyroYOffset, float *gyroZOffset) {
    float sumX = 0, sumY = 0, sumZ = 0;
    int16_t tempGyroX, tempGyroY, tempGyroZ;

    OLED_ClearArea(0, 24, 127, 63);
    
    // 显示校准开始
    OLED_ShowString(0, 24, "Calibrating...", OLED_6X8);
    OLED_Update();
      // 采集多次数据取平均值
    for(int i = 0; i < GYRO_OFFSET_SAMPLES; i++) {
        MPU6050_GetData(NULL, NULL, NULL, &tempGyroX, &tempGyroY, &tempGyroZ);
        
        // 转换为弧度/秒
        sumX += tempGyroX * 0.001065f;
        sumY += tempGyroY * 0.001065f;
        sumZ += tempGyroZ * 0.001065f;

        // 每10次采样更新一次显示
        if(i % 10 == 0) {
            OLED_ShowNum(0, 32, (i + 1) * 100 / GYRO_OFFSET_SAMPLES, 3, OLED_6X8);
            OLED_ShowString(30, 32, "%", OLED_6X8);
            OLED_Update();
        }
    
    // 计算平均值作为零偏
    *gyroXOffset = sumX / GYRO_OFFSET_SAMPLES;
    *gyroYOffset = sumY / GYRO_OFFSET_SAMPLES;
    *gyroZOffset = sumZ / GYRO_OFFSET_SAMPLES;
    }
    OLED_ClearArea(0, 24, 127, 63); // 清除校准显示
}
